#include <iostream>
#include"bezier_prediction/bezier_predict.h"
#include <ros/ros.h>
#include"api_share_python.cpp"
#include <thread>
#include <chrono>
using namespace std;

int main(){
  Bezierpredict tgpredict;
  cout<<"bezier predict launch!"<<endl;

  while(1){
    std::vector<Eigen::Vector4d> target_detect_list;

    while(share_men_instance.p_share_data->flag!=1){
      std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }

    if(share_men_instance.p_share_data->flag==1){
      for(int i = 0; i<_MAX_SEG;i++){
        target_detect_list.emplace_back(Eigen::Vector4d(share_men_instance.p_share_data->past_data[i*2], 
        share_men_instance.p_share_data->past_data[i*2+1], 1.0, i*0.1));
      }
      // for(auto p:target_detect_list)
      //   cout<<p(0)<<" "<<p(1)<<endl;
    
      int bezier_flag =0;
      bezier_flag = tgpredict.TrackingGeneration(5,5,target_detect_list);
 
      // std::vector<Eigen::Matrix<double,6,1>> predict_state_list;
      std::vector<Eigen::Vector3d> Sample_list;
      // cout<<bezier_flag<<endl;
      if(bezier_flag==0){
          // predict_state_list = tgpredict.getStateListFromBezier(_PREDICT_SEG);
          Sample_list = tgpredict.SamplePoslist_bezier(_PREDICT_SEG);
          for(int i = 0; i<_PREDICT_SEG-1;i++){
            share_men_instance.p_share_data->pred_data[i*2] = static_cast<float>(Sample_list[i+1](0));
            share_men_instance.p_share_data->pred_data[i*2+1] = static_cast<float>(Sample_list[i+1](1));
          }
          share_men_instance.p_share_data->flag = 2;
      }else{
          for(int i = 0; i<_PREDICT_SEG-1;i++){
            share_men_instance.p_share_data->pred_data[i*2] = 0;
            share_men_instance.p_share_data->pred_data[i*2+1] = 0;
          }
          share_men_instance.p_share_data->flag = 2;
      }

      // for(int i = 0; i<10;i++)
      //   cout<<share_men_instance.p_share_data->pred_data[i*2]<<" "<<share_men_instance.p_share_data->pred_data[i*2+1]<<endl;
    }

  }
 
  return 0;
}

